#include "complx_tracker.h"

#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <queue>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>



#define CAP_INDEX 1
//#define CAP_INDEX "/home/ljy/CLionProjects/VideoFile  s/WIN_20190326_21_32_49_Pro.mp4"
//#define CAP_INDEX "/media/lijiayi/OS/Users/69067/Pictures/Camera Roll/WIN_20190530_17_31_19_Pro.mp4"

using namespace cv;
using namespace std;
using namespace ros;

ofstream ViconFile, CameraFile;
Time start_time;


void ViconPosCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
    Duration dur = Time::now() - start_time;
    ViconFile << dur.toSec() << "," << msg->pose.position.x << "," << msg->pose.position.y << ","
              << msg->pose.position.z << endl;
}

int main(int argc, char *argv[]) {
    init(argc, argv, "complx_tracking");
    NodeHandle n;
//    Subscriber pos_sub=n.subscribe("/mocap/pose",10,ViconPosCallback);
    Publisher pos_pub = n.advertise<geometry_msgs::PoseStamped>("/cam/pose", 10);
    Publisher vel_pub = n.advertise<geometry_msgs::TwistStamped>("/cam/vel", 10);
    Publisher pos_raw_pub = n.advertise<geometry_msgs::PoseStamped>("/cam_raw/pose", 10);
    Publisher vel_raw_pub = n.advertise<geometry_msgs::TwistStamped>("/cam_raw/vel", 10);

    static tf::TransformBroadcaster br;


    Rate loop_rate(30);

    complx_tracker droneTracker;
    Vec3d outputRvecRaw, outputTvecRaw, outputRvec, outputTvec;
    Mat frame,showImg;

    //! [capture]
    VideoCapture capture(CAP_INDEX);
    capture.set(CAP_PROP_AUTO_EXPOSURE, 0.25);
    capture.set(CAP_PROP_EXPOSURE, 0.000001);
    if (!capture.isOpened()) {
        //error in opening the video input
        cerr << "Unable to open: " << CAP_INDEX << endl;
        return 0;
    }
    //! [capture]

    std::string pkg_path = ros::package::getPath("drone_pnp");

    CameraFile.open(pkg_path + "/data/cameraresult.csv");

//    Time last_time, current_time;
//    current_time=Time::now();
//    Vec3f last_pos, current_pos={0,0,0};

    Vec3f unfiltered_pos;
    queue<Vec3f> sliding_win;
    Vec3f sum_pos(0, 0, 0);
    geometry_msgs::PoseStamped msg_pos, msg_pos_old, msg_pos_raw, msg_pos_raw_old;
    start_time = Time::now();

    while (ros::ok()) {
        geometry_msgs::TwistStamped msg_vel;

        capture >> frame;
        if (frame.empty())
            break;

        Affine3d pose_world, pose_raw;

        if (droneTracker.apply(frame, showImg, pose_world, pose_raw)) {

            msg_pos_raw.pose.position.x = pose_raw.translation()[0];
            msg_pos_raw.pose.position.y = pose_raw.translation()[1];
            msg_pos_raw.pose.position.z = pose_raw.translation()[2];
            msg_pos_raw.header.stamp = Time::now();

            tf::Quaternion q;
            double theta = sqrt(pose_raw.rvec()[0] * pose_raw.rvec()[0] +
                                pose_raw.rvec()[1]*pose_raw.rvec()[1] +
                                pose_raw.rvec()[2]*pose_raw.rvec()[2]);

            tf::Vector3 axis = tf::Vector3(pose_raw.rvec()[0]/theta,pose_raw.rvec()[1]/theta,pose_raw.rvec()[2]/theta);
            q.setRotation(axis,theta);

            tf::Transform transform;
            transform.setOrigin(tf::Vector3(pose_raw.translation()[0],pose_raw.translation()[1],pose_raw.translation()[2]));
            transform.setRotation(q);

            br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "drone"));



            msg_pos_raw.pose.orientation.w = q.w();
            msg_pos_raw.pose.orientation.x = q.x();
            msg_pos_raw.pose.orientation.y = q.y();
            msg_pos_raw.pose.orientation.z = q.z();


            pos_raw_pub.publish(msg_pos_raw);


            ///
            ros::Duration dur_temp;
            dur_temp = Time::now() - msg_pos.header.stamp;
            if (dur_temp.toSec() > 0 && dur_temp.toSec() < 0.1) {
                msg_pos.header.stamp = Time::now();
                msg_pos.pose.position.x += (pose_world.translation()[0] - msg_pos.pose.position.x) * 0.5;
                msg_pos.pose.position.y += (pose_world.translation()[1] - msg_pos.pose.position.y) * 0.5;
                msg_pos.pose.position.z += (pose_world.translation()[2] - msg_pos.pose.position.z) * 0.5;
            } else {
                msg_pos.header.stamp = Time::now();
                msg_pos.pose.position.x = pose_world.translation()[0];
                msg_pos.pose.position.y = pose_world.translation()[1];
                msg_pos.pose.position.z = pose_world.translation()[2];
            }
            cout << msg_pos << endl;
            pos_pub.publish(msg_pos);


            /*
            unfiltered_pos = pose_world.translation();
            sliding_win.push(unfiltered_pos);
            sum_pos[0] += unfiltered_pos[0];
            sum_pos[1] += unfiltered_pos[1];
            sum_pos[2] += unfiltered_pos[2];


                if (sliding_win.size() >= 2) {
                    Vec3f t_pos;
                    t_pos = sliding_win.front();
                    sliding_win.pop();
                    sum_pos[0] -= t_pos[0];
                    sum_pos[1] -= t_pos[1];
                    sum_pos[2] -= t_pos[2];

                    msg_pos_old = msg_pos;
                    Vec3f current_pos(sum_pos[0] / sliding_win.size(), sum_pos[1] / sliding_win.size(),
                                      sum_pos[2] / sliding_win.size());

    //            last_time=current_time;
    //            current_time=Time::now();
    //            Duration dur= current_time-last_time;
    //            last_pos=current_pos;



                    msg_pos.pose.position.x = current_pos[0];
                    msg_pos.pose.position.y = current_pos[1];
                    msg_pos.pose.position.z = current_pos[2];

                    msg_pos.header.stamp = Time::now();
                    msg_vel.header.stamp = Time::now();

                    cout << msg_pos.pose.position << endl;

                    Duration dur = msg_pos.header.stamp - msg_pos_old.header.stamp;
                    msg_vel.twist.linear.x = (current_pos[0] - msg_pos_old.pose.position.x) / dur.toSec();
                    msg_vel.twist.linear.y = (current_pos[1] - msg_pos_old.pose.position.y) / dur.toSec();
                    msg_vel.twist.linear.z = (current_pos[2] - msg_pos_old.pose.position.z) / dur.toSec();

    //                cout << "Vel: " << msg_vel.twist.linear <<endl;
                    Duration time_from_start = msg_pos.header.stamp - start_time;

                    CameraFile << time_from_start.toSec() << "," << msg_pos.pose.position.x << ","
                               << msg_pos.pose.position.y << "," << msg_pos.pose.position.z;
                    CameraFile << "," << msg_vel.twist.linear.x << "," << msg_vel.twist.linear.y << ","
                               << msg_vel.twist.linear.z << endl;
                    pos_pub.publish(msg_pos);
                    vel_pub.publish(msg_vel);
                }
    //            droneTracker.virtualize();

             */

        } else {
            pos_pub.publish(msg_pos);
            msg_vel.twist.linear.x = 0;
            msg_vel.twist.linear.y = 0;
            msg_vel.twist.linear.z = 0;
            vel_pub.publish(msg_vel);
        }

//        waitKey(1);

        ros::spinOnce();
        loop_rate.sleep();
    }

    CameraFile.close();
    return 0;
}
